package org.nashua.tt151;

import com.sun.squawk.debugger.Log;
import edu.wpi.first.wpilibj.CANJaguar;
import edu.wpi.first.wpilibj.DigitalOutput;
import edu.wpi.first.wpilibj.Joystick;
import edu.wpi.first.wpilibj.Servo;

public class ActionMap {
    private HeyJude robot;
    public static Joystick driver = new Joystick(1);
    public static Joystick shooter = new Joystick(2);
    private static boolean[][] isPressed = new boolean[2][12];
    public ActionMap(HeyJude robot) {
        this.robot = robot;
    }
    public void shoot(CANJaguar st, CANJaguar sb, double power) {
        setJaguar(st,power,"Shooter Top");
        setJaguar(sb,-power,"Shooter Bottom");
    }
    /* MANUAL METHODS */
    public void manualDrive(double power, CANJaguar fl, CANJaguar fr, CANJaguar bl, CANJaguar br, boolean inverseLeft, boolean inverseRight){
        double l=driver.getRawAxis(RobotMap.DUALACTION_LEFT_Y)*(inverseLeft?-power:power);
        double r=driver.getRawAxis(RobotMap.DUALACTION_RIGHT_Y)*(inverseRight?-power:power);
        setJaguar(fl,l,"Front Left");
        setJaguar(bl,l,"Back Left");
        setJaguar(fr,r,"Front Right");
        setJaguar(bl,r,"Back Right");
    }
    public void manualAdjustShooter(Servo pitch, Servo yaw) {
        setServo(pitch,scaleValue((shooter.getRawAxis(RobotMap.DUALACTION_RIGHT_Y)+1)/2.0,RobotMap.V_SERVO_PITCH_DOWN,RobotMap.V_SERVO_DROPPER_UP),"Pitch");
        setServo(yaw,scaleValue((shooter.getRawAxis(RobotMap.DUALACTION_LEFT_X)+1)/2.0,RobotMap.V_SERVO_YAW_LEFT,RobotMap.V_SERVO_YAW_RIGHT),"Yaw");
    }
    public void manualPickup(CANJaguar pu) {
        double speed = getJaguar(pu);
        if (wasReleased(driver,RobotMap.DUALACTION_LEFT_BUMPER,1))speed=speed==-1?0:-1;
        else if (wasReleased(driver,RobotMap.DUALACTION_RIGHT_BUMPER,1))speed=speed==1?0:1;
        setJaguar(pu,speed,"Pickup");
    }
    public void manualDropper(Servo dropper){
        if (wasReleased(driver,RobotMap.DUALACTION_BUTTON_A,1)) setServo(dropper,RobotMap.V_SERVO_DROPPER_DOWN,"Dropper");
        else if(wasReleased(driver, RobotMap.DUALACTION_BUTTON_B, 1)) setServo(dropper,RobotMap.V_SERVO_DROPPER_UP,"Dropper");
    }
    public void manualShoot(Servo pitch, Servo yaw, CANJaguar st, CANJaguar sb, int hoop, DigitalOutput led){
        double power = autoAim(pitch,yaw,hoop,led);
        while (!wasReleased(shooter,RobotMap.DUALACTION_RIGHT_TRIGGER,2)) {manualAdjustShooter(pitch,yaw);}
        shoot(st,sb,power);
    }
    public void manualLED(DigitalOutput led){
        try {
            if (wasReleased(shooter,RobotMap.DUALACTION_LEFT_SELECT,2)) {led.set(true);robot.isLedOn=true;}
            else if(wasReleased(shooter, RobotMap.DUALACTION_RIGHT_SELECT, 2)) {led.set(false);robot.isLedOn=false;}
        } catch (Exception e){
            Log.log("WARNING: Failed To Set LED State - "+e.getMessage());
        }
    }
    /* AUTO METHODS */
    public void autoDrive(CANJaguar fl,CANJaguar fr, CANJaguar bl, CANJaguar br, double l, double r){
        setJaguar(fl,l,"Front Left");
        setJaguar(bl,l,"Back Left");
        setJaguar(fr,r,"Front Right");
        setJaguar(bl,r,"Back Right");
    }
    public void autoLED(DigitalOutput led, boolean state){
        led.set(state);
        robot.isLedOn=state;
    }
    public double autoAim(Servo pitch, Servo yaw, int hoop, DigitalOutput led) {
        try {
            autoLED(led, true);
            String sho = robot.dash.requestInfo("SHOOT-T"+hoop);
            pitch.set(scaleValue(Double.parseDouble(sho.substring(0,sho.indexOf(","))),RobotMap.V_SERVO_PITCH_DOWN,RobotMap.V_SERVO_PITCH_UP));
            yaw.set(scaleValue(Double.parseDouble(sho.substring(sho.indexOf(",")+1,sho.indexOf("~"))),RobotMap.V_SERVO_YAW_LEFT,RobotMap.V_SERVO_YAW_RIGHT));
            return Double.parseDouble(sho.substring(sho.indexOf("~")));
        } catch (Exception e){
            Log.log("WARNING: Failed To AutoAim - "+e.getMessage());
            return 0;
        }
    }
    public void autoPickup(CANJaguar pu, double speed) {
        setJaguar(pu,speed,"Pickup");
    }
    public void autoDropper(Servo serv, double pos) {
        setServo(serv,pos,"Dropper");
    }
    /* HELPER METHODS */
    private static void setJaguar(CANJaguar jag, double value, String id){
        try {
            jag.setX(value);
        } catch (Exception e){
            Log.log("ERROR: Setting value "+value+" on CANJaguar "+id+" failed  - "+e.getMessage());
        }
    }
    private static void setServo(Servo serv, double value, String id){
        try {
            serv.set(value);
        } catch (Exception e){
            Log.log("ERROR: Setting value "+value+" on Servo "+id+" failed  - "+e.getMessage());
        }
    }
    private static double getJaguar(CANJaguar jag){
        try {
            return jag.getX();
        } catch (Exception e){
            return 0;
        }
    }
    private static double scaleValue(double orig, double min, double max){
        return orig*(max-min)+min;
    }
    public static boolean wasReleased(Joystick j, int b, int c){
        try {
            if (isPressed[c][b]) {
                if (!j.getRawButton(b)) {
                    isPressed[c][b]=false;
                    return true;
                }
            } else if (j.getRawButton(b)) {
                isPressed[c][b]=true;
            }
        } catch (Exception  e){}
        return false;
    }
}
